/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package test.ashen;

import com.googlecode.javacv.cpp.opencv_video.CvKalman;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import static com.googlecode.javacv.cpp.opencv_core.*;
import com.googlecode.javacv.cpp.opencv_calib3d.*;
import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_video;

/**
 *
 * @author Home
 */
public class KN {

    CvKalman kf;
    CvMat state;
    CvMat pronoise;
    CvMat measurement;

    public KN() {

        state = CvMat.create(4, 1, 1111638021);
        pronoise = CvMat.create(4, 1, 1111638021);
        measurement = CvMat.create(2, 1, 1111638021);
        kf = opencv_video.cvCreateKalman(4, 2, 0);
        int i = 0;
        CvPoint po = cvPoint(0, 0);

        kf.state_pre().put(0, 0, po.x());
        kf.state_pre().put(1, 0, po.y());
        kf.state_pre().put(2, 0, 0);
        kf.state_pre().put(3, 0, 0);


        CvMat trm = CvMat.create(4, 4, 1111638021);
        trm.put(0, 0, 1);
        trm.put(0, 1, 0);
        trm.put(0, 2, 1);
        trm.put(0, 3, 0);

        trm.put(1, 0, 0);
        trm.put(1, 1, 1);
        trm.put(1, 2, 0);
        trm.put(0, 3, 1);

        trm.put(2, 0, 0);
        trm.put(2, 1, 0);
        trm.put(2, 2, 1);
        trm.put(2, 3, 0);

        trm.put(3, 0, 0);
        trm.put(3, 1, 0);
        trm.put(3, 2, 0);
        trm.put(3, 3, 1);

        kf.transition_matrix(trm);


        CvMat ms = CvMat.create(4, 2, 1111638021);

        ms.put(0, 0, 1);
        ms.put(0, 1, 0);
        ms.put(0, 2, 0);
        ms.put(0, 3, 0);


        ms.put(1, 0, 0);
        ms.put(1, 1, 1);
        ms.put(1, 2, 0);
        ms.put(1, 3, 0);

        //kf.measurement_matrix(ms);
        cvSetIdentity(state, 0);
        cvSetIdentity(kf.measurement_matrix());
        cvSetIdentity(kf.process_noise_cov(), 1e-7);
        cvSetIdentity(kf.measurement_noise_cov(), 1e-10);
        cvSetIdentity(kf.error_cov_post(), .1);


        cvRange(kf.state_post(), 0, 0.1);
    }

    public CvPoint filter(CvPoint point) {
        CvMat fpoint = CvMat.create(2, 1);

         int x=point.x();
         int y=point.y();

        CvPoint statepoint = cvPoint((int) state.get(0, 0), (int) state.get(1, 0));

        CvMat prediction = opencv_video.cvKalmanPredict(kf, null);
        //System.out.println("prediction= ("+prediction.get(0, 0)+","+prediction.get(1, 0)+")");
        CvPoint predictpoint = cvPoint((int) prediction.get(0, 0), (int) prediction.get(1, 0));
        measurement.put(0, 0, x);
        measurement.put(1, 0, y);
        //System.out.println("measurement= (" + measurement.get(0, 0) + "," + measurement.get(1, 0) + ")");

        CvMat m = CvMat.create(2, 1, kf.measurement_matrix().type());
        //cvSetIdentity(m, 0);
        cvMatMul(kf.measurement_matrix(), state, m);
        //System.out.println(m.get(0, 0));
        //System.out.println(m.get(1, 0));
        cvAdd(m, measurement, measurement, null);
        //System.out.println("measurement b4 cor= ("+measurement.get(0, 0)+","+measurement.get(1, 0)+")");

        CvMat estimated = opencv_video.cvKalmanCorrect(kf, measurement);
        //System.out.println("estimated= (" + estimated.get(0, 0) + "," + estimated.get(1, 0) + ")");
        statepoint = cvPoint((int) estimated.get(0, 0), (int) estimated.get(1, 0));

        cvRange(pronoise, 0, Math.sqrt(kf.process_noise_cov().get(0, 0)));
        cvMatMul(kf.transition_matrix(), state, state);
        cvAdd(state, pronoise, state, null);
        //System.out.println("state = ("+state.get(0,0)+","+state.get(1,0)+","+state.get(2,0)+","+state.get(3,0)+")");

        return statepoint;
    }
}
